

from Navigation.GPS import navigation
from Vission.codes.message_queue import SpeechPriorityQueue
from Vission.codes.vission import *
from Vission.codes.onnx_detect import YOLOv8Detector 
from Hardware.servo import *
from Hardware.direction_enum import Direction
from Application.utils import tts_speak
from typing import Union
import threading
import logging
import time

POSITION_PATH = "/home/elf/project_code2/Vission/modules/position.onnx"
CAR_PEOPLE_PATH = "/home/elf/project_code2/Vission/modules/person_cars.onnx"
STATIC_BARRIER_PATH = "/home/elf/project_code2/Vission/modules/static_barrier.onnx"
ROAD_STATE_PATH = "/home/elf/project_code2/Vission/modules/road_state.onnx"

logging.basicConfig(
        level=logging.INFO,
        format="%(asctime)s [%(levelname)s] %(message)s",
        handlers=[logging.FileHandler("/home/elf/project_code2/app.log"), logging.StreamHandler()]
    )

class mission:
    def __init__(self,speaker,city=None, targetname=None,mode="navigation"):
        self.message_queue=SpeechPriorityQueue()
        self.create_detecters()
        self.speaker=speaker
        self.cap=open_camera()
        setup_pwm()
        

        if mode=="navigation":
            self.navigation = navigation(city, targetname)
            self.navigation.navi()
            routes = self.navigation.navigation_steps or []
            routes.append("到达目的地")
            routes_lengths = self.navigation.routes_length or []
            routes_lengths.append(0)
            self.tasks = [(route, route_length) for route, route_length in zip(routes, routes_lengths)]
            self.current_target_length = 0                      #当前分段的目标距离
            self.real_distance = 0                              #当前分段已行走的距离
            self.navi_thread = threading.Thread(target=self.navi_task, daemon=True)
            self.arrive_at_crossroad_event = threading.Event()
            self.arrive_at_distance_event = threading.Event()
        
        self.current_direction=Direction.FRONT               #当前摄像头的方向
        direction(self.current_direction.value)
        self.current_direction_lock=threading.Lock()
        self.vission_thread = threading.Thread(target=self.vission_task, daemon=True)
        self.speaker_thread = threading.Thread(target=self.speaker_task,daemon=True)
        self.mode=mode

        self.run()

    def run(self):
        self.thread_start()

    def stop(self):
        message="导航结束，请再次呼唤小杰唤醒"
        self.message_queue.push(speech_message(priority=Priority.Text, subpriority=Subpriority.A, message=message))
        self.vission_thread.join(timeout=1)
        self.speaker_thread.join(timeout=1)
        if self.mode=="navigation":
            self.navi_thread.join(timeout=1)
            self.navigation.gps_get.stop()
        release_camera(self.cap)
        logging.info("资源释放完毕")


    def thread_start(self):
        """
        开启所有线程，
        将初始的信号量初始化好
        """
        if self.mode=="navigation":
            self.navi_thread.start()
            self.arrive_at_crossroad_event.set()
            self.arrive_at_distance_event.set()
        
        self.vission_thread.start()
        self.speaker_thread.start()
        logging.info("线程初始化成功")

    def create_detecters(self):
        """
        为多个模型的调用进行初始准备
        """
        self.position_detecter=YOLOv8Detector(onnx_path=POSITION_PATH,
                                                class_names=['zebra_crossing', 'crossroad', 'road', 
                                                               'street', 'stairs', 'left t-junction', 't-junction'])
        self.car_people_detecter=YOLOv8Detector(onnx_path=CAR_PEOPLE_PATH,
                                                class_names=["car_left_moving","car_right_moving",  "car_front_approaching","car_front_moving_left", 
                                                                "car_front_moving_right","car_rear_approaching","pedestrian_front","pedestrian_right","pedestrian_left" ])
        self.static_barrier_detecter=YOLOv8Detector(onnx_path=STATIC_BARRIER_PATH,
                                                class_names=["Static-front", "Static-right", "Safe-path-left", "Static-left", "Safe-path-front", "Safe-path-right"])
        self.roadstate_detecter=YOLOv8Detector(onnx_path=ROAD_STATE_PATH,
                                                class_names=["narrow","wide","complex"])
        logging.debug("[application] detecters 初始化成功")

    
    def navi_task(self):
        """
        逐一播报路线
        每到达一个节点开始播报下一个
        """
        try:
            logging.info(f"TASK:{self.tasks}")
            for task in self.tasks:
                logging.info(task)
                self.arrive_at_distance_event.wait()
                self.arrive_at_crossroad_event.wait()
                self.navigation.previouspos=self.navigation.currentpos.copy()
                self.current_target_length = task[1]
                route_instruction = task[0]
                message=route_instruction
                self.message_queue.push(speech_message(priority=Priority.Text, subpriority=Subpriority.A, message=message))
                logging.info(f"[navi thread] 下一步：{route_instruction}")
                self.arrive_at_crossroad_event.clear()
                self.arrive_at_distance_event.clear()
                
            logging.info("[navi thread] 到达目的地，导航完成")
        finally:
            self.stop()
            

    def speaker_task(self):
        """
        语音播报
        """
        logging.info("[speak thread] 播报线程成功开启")
        while True:
            if  not self.message_queue.is_empty():
                message=self.message_queue.pop()
                tts_speak(message.message)
                time.sleep(1.5)
        

    def vission_task(self, debug=False):
        SCAN_INTERVAL_HIGH = 2.5  # 高频扫描间隔(秒)
        SCAN_INTERVAL_LOW = 15    # 低频扫描间隔(秒)
        DETECTION_INTERVAL = 0.3  # 基础检测间隔(秒)
        last_navi_time=time.time()
        last_scan_time = time.time()
        last_detection_time = time.time()
        
        while True:
            current_time = time.time()
            img = get_img(self.cap)
            if img is None:
                time.sleep(0.1)
                continue
                
            # 位置检测
            if self.current_direction.value !=0:
                direction(0)
                time.sleep(0.5)
            position = detect_position(self, img)
            if debug and position is None:
                position = 'crossroad'
                
            
                
            if self.mode=="navigation" and (time.time()-last_navi_time)>5:
                if self.navigation.Calculate_GPS_Distance != -1:
                    self.navigation.get_currentpos()
                    last_navi_time=time.time()
                    self.real_distance = self.navigation.Calculate_GPS_Distance
                    logging.info(f"[navi] 当前距离: {self.real_distance:.2f} 米，目标: {self.current_target_length} 米")
                    if abs(self.real_distance - self.current_target_length) <= 20:
                        self.arrive_at_distance_event.set()
                        logging.info("[navi] 已经到了阶段终点附近")
                        if debug:
                            self.arrive_at_crossroad_event.set()
                            logging.info("[navi] 到达小地点，触发下一段导航")
                            time.sleep(3)
                        else :
                            if position in ['zebra_crossing', 'crossroad', 'left t-junction', 't-junction']:
                                self.arrive_at_crossroad_event.set()
                                logging.info("[navi] 到达小地点，触发下一段导航")
                                time.sleep(3)

            if position is None:
                time.sleep(0.5)
                continue
            
            # 改进后的扫描逻辑
            if position in ['zebra_crossing', 'crossroad', 'left t-junction', 't-junction']:
                # 高频扫描模式
                if current_time - last_scan_time >= SCAN_INTERVAL_HIGH:
                    with self.current_direction_lock:
                        self.current_direction = (self.current_direction + 1) % 4
                    direction(self.current_direction.value)
                    last_scan_time = current_time
                    
                # 高频检测(所有方向)
                if current_time - last_detection_time >= DETECTION_INTERVAL:
                    detect_car_people(self, img)
                    last_detection_time = current_time
                    
            else:  # 普通道路/街道
                # 低频扫描模式
                if current_time - last_scan_time >= SCAN_INTERVAL_LOW:
                    with self.current_direction_lock:
                        self.current_direction = (self.current_direction + 1) % 4
                    direction(self.current_direction.value)
                    last_scan_time = current_time
                    
                # 仅正面方向检测
                if self.current_direction == Direction.FRONT:
                    if current_time - last_detection_time >= DETECTION_INTERVAL:
                        detect_car_people(self, img)
                        detect_static_barrier(img, self.static_barrier_detecter, self.message_queue)
                        last_detection_time = current_time
            
            # 固定间隔休眠，降低CPU占用
            time.sleep(0.1)

            if position in ['zebra_crossing', 'crossroad', 'left t-junction', 't-junction']:
                logging.info("[vission thread] 高频扫描模式，每10秒360°旋转")
                start_time = time.time()
                current_time=time.time()
                last_scan_time=time.time()
                while (current_time - start_time) < 60:
                    current_time = time.time()
                    img = get_img(self.cap)
                    if img is None:
                        continue
                    detect_car_people(self, img)
                    if (time.time()-last_scan_time)>2.5:#10秒钟一圈
                        with self.current_direction_lock:
                            self.current_direction=(self.current_direction+1)%4
                        direction(self.current_direction.value)
                        time.sleep(0.5)
                        last_scan_time=time.time()
                    time.sleep(0.5)
                        
                        

            if position in ["street", "stairs", "road"]:
                logging.info("[vission thread] 低频扫描模式，每60秒360°旋转")
                current_time2 = time.time()
                last_scan_time2 = time.time()
                start_time2=time.time()
                while (current_time2 - start_time2) < 60:
                    current_time2 = time.time()
                    img = get_img(self.cap)
                    if img is None:
                        continue
                    detect_car_people(self, img)
                    if self.current_direction == Direction.FRONT:
                        detect_static_barrier(img, self.static_barrier_detecter, self.message_queue)
                    
                    if (time.time()-last_scan_time2)>15:#一分钟一圈
                        with self.current_direction_lock:
                            self.current_direction=(self.current_direction+1)%4
                        direction(self.current_direction.value)
                        time.sleep(0.5)
                        last_scan_time2=time.time()
                        
                    time.sleep(1)  
                    
                



    



